#include <unistd.h>
#include <error.h>
#include <errno.h>
#include <fcntl.h>
#include <sys/ioctl.h>
#include <sys/types.h>
#include <pthread.h>
#include <linux/videodev2.h>
#include <sys/mman.h>
#include <stdio.h>
#include <stdlib.h>
#include <iostream>

#include "stream_puller_usb.h"

namespace stream {


HBStreamPullerUSB::HBStreamPullerUSB(StreamPullerInfo* spinfo, LogInfo* log_info): 
  HBStreamPullerBase(spinfo, log_info){

  this->devName = puller_ifo->streamAddress;
  this->fd_cam = -1;
  this->buffers = NULL;
  this->n_buffers = 0;
  this->frameIndex = -1;

}

HBStreamPullerUSB::~HBStreamPullerUSB() {

}


int HBStreamPullerUSB::openDevice() {
  /*设备的打开*/
  printf("video dev : %s\n", puller_ifo->streamAddress.c_str());
  fd_cam = open(puller_ifo->streamAddress.c_str(), O_RDWR);
  if (fd_cam < 0) {
    perror("Can't open video device");
    return fd_cam;
  }
  return 0;
}

int HBStreamPullerUSB::closeDevice() {
  if (fd_cam > 0) {
    int ret = 0;
    if ((ret = close(fd_cam)) < 0) {
      perror("Can't close video device");
    }
    return 0;
  } else {
    return -1;
  }
}

int HBStreamPullerUSB::initDevice() {
  int ret;
  struct v4l2_capability cam_cap;    //显示设备信息
  struct v4l2_cropcap cam_cropcap;  //设置摄像头的捕捉能力
  struct v4l2_fmtdesc cam_fmtdesc;  //查询所有支持的格式：VIDIOC_ENUM_FMT
  struct v4l2_crop cam_crop;      //图像的缩放
  struct v4l2_format cam_format;    //设置摄像头的视频制式、帧格式等

  /* 使用IOCTL命令VIDIOC_QUERYCAP，获取摄像头的基本信息*/
  ret = ioctl(fd_cam, VIDIOC_QUERYCAP, &cam_cap);
  if (ret < 0) {
    perror("Can't get device information: VIDIOCGCAP");
  }
  printf(
      "Driver Name:%s\nCard Name:%s\nBus info:%s\nDriver Version:%u.%u.%u\n",
      cam_cap.driver, cam_cap.card, cam_cap.bus_info,
      (cam_cap.version >> 16) & 0XFF, (cam_cap.version >> 8) & 0XFF,
      cam_cap.version & 0XFF);

  /* 使用IOCTL命令VIDIOC_ENUM_FMT，获取摄像头所有支持的格式*/
  cam_fmtdesc.index = 0;
  cam_fmtdesc.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
  printf("Support format:\n");
  while (ioctl(fd_cam, VIDIOC_ENUM_FMT, &cam_fmtdesc) != -1) {
    printf("\t%d.%s\n", cam_fmtdesc.index + 1, cam_fmtdesc.description);
    cam_fmtdesc.index++;
  }

  /* 使用IOCTL命令VIDIOC_CROPCAP，获取摄像头的捕捉能力*/
  cam_cropcap.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
  if (0 == ioctl(fd_cam, VIDIOC_CROPCAP, &cam_cropcap)) {
    printf("Default rec:\n\tleft:%d\n\ttop:%d\n\twidth:%d\n\theight:%d\n",
        cam_cropcap.defrect.left, cam_cropcap.defrect.top,
        cam_cropcap.defrect.width, cam_cropcap.defrect.height);
    /* 使用IOCTL命令VIDIOC_S_CROP，获取摄像头的窗口取景参数*/
    cam_crop.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
    cam_crop.c = cam_cropcap.defrect;    //默认取景窗口大小
    if (-1 == ioctl(fd_cam, VIDIOC_S_CROP, &cam_crop)) {
      //printf("Can't set crop para\n");
    }
  } else {
    printf("Can't set cropcap para\n");
  }

  /* 使用IOCTL命令VIDIOC_S_FMT，设置摄像头帧信息*/
  cam_format.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
  cam_format.fmt.pix.width = puller_ifo->oriWidth;
  cam_format.fmt.pix.height = puller_ifo->oriHeight;
  cam_format.fmt.pix.pixelformat = V4L2_PIX_FMT_MJPEG;    //要和摄像头支持的类型对应
  cam_format.fmt.pix.field = V4L2_FIELD_INTERLACED;
  ret = ioctl(fd_cam, VIDIOC_S_FMT, &cam_format);
  if (ret < 0) {
    perror("Can't set frame information");
  }
  /* 使用IOCTL命令VIDIOC_G_FMT，获取摄像头帧信息*/
  cam_format.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
  ret = ioctl(fd_cam, VIDIOC_G_FMT, &cam_format);
  if (ret < 0) {
    perror("Can't get frame information");
  }
  printf("Current data format information:\n\twidth:%d\n\theight:%d\n",
      cam_format.fmt.pix.width, cam_format.fmt.pix.height);
  ret = initBuffers();
  if (ret < 0) {
    perror("Buffers init error");
    //exit(-1);
  }
  return 0;
}

int HBStreamPullerUSB::initBuffers() {
  int ret;
  /* 使用IOCTL命令VIDIOC_REQBUFS，申请帧缓冲*/
  struct v4l2_requestbuffers req;
  CLEAR(req);
  req.count = 4;
  req.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
  req.memory = V4L2_MEMORY_MMAP;
  ret = ioctl(fd_cam, VIDIOC_REQBUFS, &req);
  if (ret < 0) {
    perror("Request frame buffers failed");
  }
  if (req.count < 2) {
    perror("Request frame buffers while insufficient buffer memory");
  }
  buffers = (struct cam_buffer*) calloc(req.count, sizeof(*buffers));
  if (!buffers) {
    perror("Out of memory");
  }
  for (n_buffers = 0; n_buffers < req.count; n_buffers++) {
    struct v4l2_buffer buf;
    CLEAR(buf);
    // 查询序号为n_buffers 的缓冲区，得到其起始物理地址和大小
    buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
    buf.memory = V4L2_MEMORY_MMAP;
    buf.index = n_buffers;
    ret = ioctl(fd_cam, VIDIOC_QUERYBUF, &buf);
    if (ret < 0) {
      printf("VIDIOC_QUERYBUF %d failed\n", n_buffers);
      return -1;
    }
    buffers[n_buffers].length = buf.length;
    //printf("buf.length= %d\n",buf.length);
    // 映射内存
    buffers[n_buffers].start = mmap(
        NULL, // start anywhere
        buf.length, PROT_READ | PROT_WRITE, MAP_SHARED, fd_cam,
        buf.m.offset);
    if (MAP_FAILED == buffers[n_buffers].start) {
      printf("mmap buffer%d failed\n", n_buffers);
      return -1;
    }
    printf("initBuffers %d.\n", n_buffers);
  }
  return 0;
}

int HBStreamPullerUSB::startCapture() {
  unsigned int i;
  for (i = 0; i < n_buffers; i++) {
    struct v4l2_buffer buf;
    CLEAR(buf);
    buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
    buf.memory = V4L2_MEMORY_MMAP;
    buf.index = i;
    if (-1 == ioctl(fd_cam, VIDIOC_QBUF, &buf)) {
      printf("VIDIOC_QBUF buffer%d failed\n", i);
      return -1;
    }
    printf("VIDIOC_QBUF buffer%d ok.\n", i);
  }
  enum v4l2_buf_type type;
  type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
  if (-1 == ioctl(fd_cam, VIDIOC_STREAMON, &type)) {
    printf("VIDIOC_STREAMON error\n");
    return -1;
  }
  printf("VIDIOC_STREAMON ok\n");
  return 0;
}

int HBStreamPullerUSB::stopCapture() {
  enum v4l2_buf_type type;
  type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
  if (-1 == ioctl(fd_cam, VIDIOC_STREAMOFF, &type)) {
    printf("VIDIOC_STREAMOFF error\n");
    return -1;
  }
  return 0;
}

int HBStreamPullerUSB::freeBuffers() {
  unsigned int i;
  for (i = 0; i < n_buffers; ++i) {
    if (-1 == munmap(buffers[i].start, buffers[i].length)) {
      printf("munmap buffer%d failed\n", i);
      return -1;
    }
  }
  free(buffers);
  return 0;
}

int HBStreamPullerUSB::get_frame(stream::ImageBlob& buffer) {
  int ret = 0;
  queue_lock.lock();
  if (queue_data.empty()) { queue_lock.unlock(); return -1; }
  buffer = queue_data.front();
  queue_data.pop();
  queue_lock.unlock();
  return 0;
}
int HBStreamPullerUSB::start_pull_imp() {
  int ret = 0;

  // 初始化channel属性
  VDEC_CHN_ATTR_S vdecChnAttr;
  ret = sample_vdec_ChnAttr_init(&vdecChnAttr);
  if (ret != 0) { spdlog::get("logger")->info("ERROR. sample_venc_ChnAttr_init failded: {}\n", ret);}
  // 创建channel
  ret = HB_VDEC_CreateChn(puller_ifo->vdecChn, &vdecChnAttr);
  if (ret != 0) { spdlog::get("logger")->info("ERROR. HB_VDEC_CreateChn {} failed, {}.\n", puller_ifo->vdecChn, ret);}
  // 设置channel属性
  ret = HB_VDEC_SetChnAttr(puller_ifo->vdecChn, &vdecChnAttr);  // config
  if (ret != 0) { spdlog::get("logger")->info("ERROR. HB_VDEC_SetChnAttr failed {}\n", ret);}

  ret = HB_VDEC_StartRecvStream(puller_ifo->vdecChn);
  if (ret != 0) { spdlog::get("logger")->info("ERROR. HB_VDEC_StartRecvStream failed {}\n", ret);}


  std::thread(&HBStreamPullerUSB::start_pull_listener, this).detach();
  std::this_thread::sleep_for(std::chrono::milliseconds(3000));

  int size_y = puller_ifo->oriWidth * puller_ifo->oriHeight;
  int size_uv = size_y / 2;
  stream::ImageBlob buffer(stream::ImageBlobMode_YUV, size_y+size_uv);
  buffer.ori_im_shape = {puller_ifo->oriHeight, puller_ifo->oriWidth};
  buffer.yuv_size = size_y+size_uv;

  int frame_cnt = 0;
  VIDEO_FRAME_S stFrameInfo;
  // int64_t timepoint = duration_cast<milliseconds>(system_clock::now().time_since_epoch()).count();;
  int idx = 0;
  float vdec_fps = 0;
  int64_t cur_time = 0;
  int vdec_cnt = 0;
  int64_t start_time = 0, end_time = 0;
  stop_flag = common::CameraGrabing;
  while (stop_flag == common::CameraGrabing) {
    std::this_thread::sleep_for(std::chrono::milliseconds(3));
    ret = HB_VDEC_GetFrame(puller_ifo->vdecChn, &stFrameInfo, 1000);
    // printf("HB_VDEC_GetFrame:%d\n", ret);
    if (ret != 0) { printf("HB_VDEC_GetFrame failed:%d\n", ret); continue; }
    vdec_cnt++;

    cur_time = duration_cast<milliseconds>(system_clock::now().time_since_epoch()).count();
    std::copy(stFrameInfo.stVFrame.vir_ptr[0], stFrameInfo.stVFrame.vir_ptr[0] + size_y, buffer.yuv.begin());
    std::copy(stFrameInfo.stVFrame.vir_ptr[1], stFrameInfo.stVFrame.vir_ptr[1] + size_uv, buffer.yuv.begin()+size_y);
    if (idx % 300 == 0) {printf("vdec %d, %dx%d, %lld\n", idx++, stFrameInfo.stVFrame.width, stFrameInfo.stVFrame.height, cur_time);}

    HB_VDEC_ReleaseFrame(puller_ifo->vdecChn, &stFrameInfo);

    queue_lock.lock();
    if (queue_data.size() >= buffer_cnt) {
      queue_data.pop();
      printf("HB_VDEC_GetFrame pop. max_size:3.\n");
    }
    queue_data.push(buffer);
    queue_lock.unlock();

    // int64_t cur_time = duration_cast<milliseconds>(system_clock::now().time_since_epoch()).count();;
    // std::cout << frame_cnt++ << ".  decoder time: " << time_1 - timepoint << ".  total time: " << cur_time - timepoint << std::endl;
    // timepoint = cur_time;

    // 输出fps
    end_time = duration_cast<milliseconds>(system_clock::now().time_since_epoch()).count();
    if (end_time - start_time > 5 * 1e3) {
      vdec_fps = 1.0 / ((end_time - start_time) / (vdec_cnt * 1e3));
      printf("vdec_fps c:%d : %f, vdec_cnt:%d\n", puller_ifo->vdecChn, vdec_fps, vdec_cnt);
      spdlog::get("logger")->info("RKStreamPullerNET::start_pull. vdec_fps c:{} : {}, vdec_cnt:{}", puller_ifo->channelId, vdec_fps, vdec_cnt);
      vdec_cnt = 0;
      start_time = end_time;
    }
  }
  
  // free
  while (!queue_data.empty()) { queue_data.pop(); }
  stop_flag = common::CameraClosed;
  return 0;

}

int HBStreamPullerUSB::getFrame(void **frame_buf, size_t* len) {
  struct v4l2_buffer queue_buf;
  CLEAR(queue_buf);
  queue_buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
  queue_buf.memory = V4L2_MEMORY_MMAP;
  if (-1 == ioctl(fd_cam, VIDIOC_DQBUF, &queue_buf)) {
    printf("VIDIOC_DQBUF error\n");
    return -1;
  }
  *frame_buf = buffers[queue_buf.index].start;
  *len = buffers[queue_buf.index].length;
  frameIndex = queue_buf.index;
  return 0;
}

int HBStreamPullerUSB::backFrame() {
  if (frameIndex != -1) {
    struct v4l2_buffer queue_buf;
    CLEAR(queue_buf);
    queue_buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
    queue_buf.memory = V4L2_MEMORY_MMAP;
    queue_buf.index = frameIndex;
    if (-1 == ioctl(fd_cam, VIDIOC_QBUF, &queue_buf)) {
      printf("VIDIOC_QBUF error\n");
      return -1;
    }
    return 0;
  }
  return -1;
}

int HBStreamPullerUSB::init_params() {
  int ret = 0;
  // init
  if (0 != (ret = openDevice())) {return ret;}
  ret = initDevice();
  ret = startCapture();
  if (ret != 0) {return ret;}
  return 0;
}


int HBStreamPullerUSB::start_pull() {
  int ret = 0;

  start_pull_imp();
  return 0;

}

int HBStreamPullerUSB::start_pull_listener() {
  int ret = 0;
  // stop_flag2 = common::CameraGrabing;
  // while (stop_flag2 == common::CameraGrabing) {
    ret = start_pull_listener_imp();
  // }
  // stop_flag2 = common::CameraOpened;
  return ret;
}

int HBStreamPullerUSB::start_pull_listener_imp() {
  int ret = 0;

  // 准备buffer
  int mmz_cnt = 3;
  uint64_t mmz_paddr[mmz_cnt];
  memset(mmz_paddr, 0, sizeof(mmz_paddr));
  char* mmz_vaddr[mmz_cnt];
  int mmz_size = puller_ifo->oriWidth * puller_ifo->oriHeight * 4;
  // int mmz_size = puller_ifo->oriWidth * puller_ifo->oriHeight / 4;
  for (int i = 0; i < mmz_cnt; i++) {
    mmz_vaddr[i] = nullptr;
    ret = HB_SYS_Alloc(&(mmz_paddr[i]), (void **)&mmz_vaddr[i], mmz_size);
    // printf("mmzAlloc paddr = 0x%lx, vaddr = 0x%lx i = %d, ret=%d \n", (uint64_t)mmz_paddr[i], (uint64_t)mmz_vaddr[i],i, ret); 
    if (ret != 0) { spdlog::get("logger")->info("mmzAlloc paddr = 0x{}, vaddr = 0x{} i = {}, ret={} \n", (uint64_t)mmz_paddr[i], (uint64_t)mmz_vaddr[i],i, ret);}
  }
  VIDEO_STREAM_S pstStream;
  memset(&pstStream, 0, sizeof(VIDEO_STREAM_S));
  
  unsigned char *yuv422frame = NULL;
  unsigned long yuvframeSize = 0;

  float pull_fps;
  int pts_count = 0, fps_cnt = 0;
  int64_t start_time = 0, end_time = 0;
  int mmz_index = 0;
  VDEC_CHN_STATUS_S pstStatus;
  int av_error=0;
  stop_flag2 = common::CameraGrabing;
  while (stop_flag2 == common::CameraGrabing) {

    ret = HB_VDEC_QueryStatus(puller_ifo->vdecChn, &pstStatus);
    if (ret != 0) { spdlog::get("logger")->info("ERROR. HB_VDEC_QueryStatus error:{}\n", ret); break;}
    if (pstStatus.cur_input_buf_cnt >= (uint32_t)mmz_cnt) {
      std::this_thread::sleep_for(milliseconds(1));
      continue;
    }

    getFrame((void **) &yuv422frame, (size_t *)&yuvframeSize);
    if (yuvframeSize >= mmz_size) { continue; }
    mmz_index = pts_count % mmz_cnt;
    
    memcpy((void*)mmz_vaddr[mmz_index], (void*)yuv422frame, yuvframeSize);
    pstStream.pstPack.phy_ptr = mmz_paddr[mmz_index];
    pstStream.pstPack.vir_ptr = mmz_vaddr[mmz_index];
    pstStream.pstPack.pts = pts_count++;
    pstStream.pstPack.src_idx = mmz_index;
    pstStream.pstPack.size = yuvframeSize;
    pstStream.pstPack.stream_end = HB_FALSE;
    fps_cnt++;
    
    ret = HB_VDEC_SendStream(puller_ifo->vdecChn, &pstStream, 3000);
    if (ret == -HB_ERR_VDEC_OPERATION_NOT_ALLOWDED || ret == -HB_ERR_VDEC_UNKNOWN) {
      spdlog::get("logger")->info("ERROR. HB_VDEC_SendStream failed\n");
    }

    backFrame();

    // 输出fps
    end_time = duration_cast<milliseconds>(system_clock::now().time_since_epoch()).count();
    if (end_time - start_time > 5 * 1e3) {
      pull_fps = 1.0 / ((end_time - start_time) / (fps_cnt * 1e3));
      // spdlog::get("logger")->info("pull_fps: {}", pull_fps);
      printf("pull_fps c:%d : %f\n", puller_ifo->vdecChn, pull_fps);
      start_time = end_time;
      fps_cnt = 0;
    }
  }


  stopCapture();
  freeBuffers();
  closeDevice();

  HB_VDEC_DestroyChn(puller_ifo->vdecChn);
  for (int i = 0; i < mmz_cnt; i++) {
    ret = HB_SYS_Free(mmz_paddr[i], mmz_vaddr[i]);
    // printf("mmzFree paddr = 0x%lx, vaddr = 0x%lx i = %d, ret=%d\n", (uint64_t)mmz_paddr[i], (uint64_t)mmz_vaddr[i], i, ret);
    if (ret != 0) { spdlog::get("logger")->info("mmzFree paddr = 0x{}, vaddr = 0x{} i = {}, ret={}\n", (uint64_t)mmz_paddr[i], (uint64_t)mmz_vaddr[i], i, ret);}
  }

  stop_flag2 = common::CameraClosed;
  return 0;
}



}